#pragma once

#include "i_base.h"

namespace idl
{
  template <typename KEYPOINT_TYPE>
  class MultiScaleInterestPointPyramid
  {
  public:
    MultiScaleInterestPointPyramid();
    MultiScaleInterestPointPyramid(const MultiScaleInterestPointPyramid<KEYPOINT_TYPE>& pyramid);
    MultiScaleInterestPointPyramid& operator=(const MultiScaleInterestPointPyramid<KEYPOINT_TYPE>& pyramid);   
    virtual ~MultiScaleInterestPointPyramid();
    bool init(int max_base_scale_capacity, int nr_scale, int downsample_factor = 4);
    const KEYPOINT_TYPE* const_data() const { return (const KEYPOINT_TYPE*)_memory; };
    const KEYPOINT_TYPE* const_data(unsigned int scale) const;
    KEYPOINT_TYPE* data() { return _memory; };
    KEYPOINT_TYPE* data(unsigned int scale);
    KEYPOINT_TYPE& operator()(unsigned int scale, unsigned int index)
    {
      assert(scale < _nr_scale && index < _capacity[scale]);
      return _data[scale][index];
    }
    const KEYPOINT_TYPE& operator()(unsigned int scale, unsigned int index) const
    {
      assert(scale < _nr_scale && index < _capacity[scale]);
      return _data[scale][index];
    }
    bool initialized() const { return _initialized; };
    int nr_scale() const { return _nr_scale; };
    int size(unsigned int scale) const;
    int size() const;
    int capacity(unsigned int scale) const;
    int capacity() const;
    int cdsf() const { return _cdsf; };
    void reset(const KEYPOINT_TYPE& value);
    void reset(unsigned int scale, const KEYPOINT_TYPE& value);
    bool increase_size(unsigned int scale);
    bool set_size(unsigned int scale, int size);
    void clear();
    void clear(unsigned int scale);
  private:
    void cleanup();
  private:
    bool _initialized;
    int _nr_scale, _cdsf/*capacity downsample factor between scales, now it is an integer*/;
    std::vector<int> _capacity;
    std::vector<int> _nr_ipts;
    std::vector<KEYPOINT_TYPE*> _data;
    KEYPOINT_TYPE* _memory;
  };

  template <typename KEYPOINT_TYPE>
  MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::MultiScaleInterestPointPyramid() :_initialized(false), _nr_scale(0), _cdsf(1), _memory(NULL){}

  template <typename KEYPOINT_TYPE>
  MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::MultiScaleInterestPointPyramid(const MultiScaleInterestPointPyramid<KEYPOINT_TYPE>& pyramid)
  {
    int i, offset, capacity = 0;
    _initialized = false;
    _nr_scale = 0;
    _cdsf = 1;
    _memory = NULL;

    if (pyramid.initialized())
    {
      _nr_scale = pyramid.nr_scale();
      _cdsf = pyramid.cdsf();
      _capacity.resize(_nr_scale, 0);
      _nr_ipts.resize(_nr_scale, 0);
      _data.resize(_nr_scale, (KEYPOINT_TYPE*)NULL);

      for (i = 0; i < _nr_scale; ++i)
      {
        _capacity.at(i) = pyramid.capacity(i);
        _nr_ipts.at(i) = pyramid.size(i);
      }
      
      _memory = i_alloc<KEYPOINT_TYPE>(pyramid.capacity());

      if (_memory)/*allocated enough system memory*/
      {
        i_copy(pyramid.const_data(), _memory, pyramid.capacity());
        _data[0] = _memory;
        for (i = 1; i < _nr_scale; ++i)
        {
          _data[i] = _data[i-1] + _capacity[i-1];
        }
        _initialized = true;
      }/*if memory != NULL*/
      else
      {
        cleanup(); /*cleanup if memory allocation failed*/
      }
    }/*if pyramid.initialized() == TRUE*/
  }

  template <typename KEYPOINT_TYPE>
  MultiScaleInterestPointPyramid<KEYPOINT_TYPE>& MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::operator=(const MultiScaleInterestPointPyramid<KEYPOINT_TYPE>& pyramid)
  {
    if (this == &pyramid)/*check self-assignment*/
    {
      return(*this);
    }
    if (this->init(pyramid.capacity(0), pyramid.nr_scale(), pyramid.cdsf()))
    {
      i_copy<KEYPOINT_TYPE>(pyramid.const_data(), this->_memory, pyramid.capacity());
    }
    return(*this);
  }

  template <typename KEYPOINT_TYPE>
  MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::~MultiScaleInterestPointPyramid()
  {
    cleanup();
  }

  template <typename KEYPOINT_TYPE>
  bool MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::init(int max_base_scale_capacity, int nr_scale, int downsample_factor)
  {
    int i, length = 0, capacity = max_base_scale_capacity; /*no memory copy for the first scale, using the original image data address*/
    
    if (!max_base_scale_capacity || !nr_scale || !downsample_factor)
    {
      return false;
    }

    if (_initialized /*memory has been alloced*/)
    {
      if (nr_scale != _nr_scale || max_base_scale_capacity != _capacity.front() || downsample_factor != _cdsf)
      {
        cleanup();
      }
      else
      {
        return true;
      }
    }

    for (i = 0; i < nr_scale; ++i)
    {
      if (capacity <= 0/*min. feature capacity is 1*/)
      {
        break;
      }
      _capacity.push_back(capacity);
      length += capacity;
      capacity = i_div(capacity, downsample_factor);
    }

    if (_capacity.empty())
    {
      cleanup();
      return false;
    }
    
    _memory = i_alloc<KEYPOINT_TYPE>(length);
    
    if (_memory == NULL)
    {
      cleanup();
      return false;
    }

    if (_memory)/*allocated enough system memory*/
    {
      _nr_scale = (int)_capacity.size();/*actual # of scales*/
      _data.resize(_nr_scale, (KEYPOINT_TYPE*)NULL);
      _nr_ipts.resize(_nr_scale, 0);
      _cdsf = downsample_factor;

      _data[0] = _memory;
      for (i = 1; i < _nr_scale; ++i)
      {
        _data[i] = _data[i - 1] + _capacity[i - 1];
      }
      _initialized = true;
    }/*if memory != NULL*/
    else
    {
      cleanup(); /*cleanup if memory allocation failed*/
    }
    
    return _initialized;
  }

  template <typename KEYPOINT_TYPE>
  void MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::cleanup()
  {
    i_free<KEYPOINT_TYPE>(_memory);
    _data.clear();
    _capacity.clear();
    _nr_ipts.clear();
    _nr_scale = 0;
    _cdsf = 1;
    _initialized = false;
  }

  template <typename KEYPOINT_TYPE>
  KEYPOINT_TYPE* MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::data(unsigned int scale)
  {
    return (scale < (unsigned int)_nr_scale) ? _data[scale] : NULL;
  }

  template <typename KEYPOINT_TYPE>
  const KEYPOINT_TYPE* MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::const_data(unsigned int scale) const
  {
    return (scale < (unsigned int)_nr_scale) ? (const KEYPOINT_TYPE*)_data[scale] : NULL;
  }

  template <typename KEYPOINT_TYPE>
  int MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::size(unsigned int scale) const
  {
    return (scale < (unsigned int)_nr_scale) ? _nr_ipts[scale] : 0;
  }

  template <typename KEYPOINT_TYPE>
  int MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::size() const
  {
    int nr = 0;
    for (int i = 0; i < _nr_scale; ++i)
    {
      nr += _nr_ipts[i];
    }
    return nr;
  }

  template <typename KEYPOINT_TYPE>
  int MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::capacity(unsigned int scale) const
  {
    return (scale < (unsigned int)_nr_scale) ? _capacity[scale] : 0;
  }

  template <typename KEYPOINT_TYPE>
  int MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::capacity() const
  {
    int capacity = 0;
    for (int i = 0; i < _nr_scale; ++i)
    {
      capacity += _capacity[i];
    }
    return capacity;
  }

  template <typename KEYPOINT_TYPE>
  void MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::reset(const KEYPOINT_TYPE& value)
  {
    int i, capacity = this->capacity();
    for (i = 0; i < capacity; i++)
    {
      _memory[i] = value;
    }
  }
  
  template <typename KEYPOINT_TYPE>
  void MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::reset(unsigned int scale, const KEYPOINT_TYPE& value)
  {
    int i, capacity = this->capacity(scale);
    for (i = 0; i < capacity; i++)
    {
      _data[scale][i] = value;
    }
  }

  template <typename KEYPOINT_TYPE>
  bool MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::increase_size(unsigned int scale)
  {
    bool ret = false;
    if (scale  < (unsigned int)_nr_scale)
    {
      if (_nr_ipts[scale] < _capacity[scale])
      {
        _nr_ipts[scale] = _nr_ipts[scale] + 1;
        ret = true;
      }
    }
    return ret;
  }
  
  template <typename KEYPOINT_TYPE>
  bool MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::set_size(unsigned int scale, int size)
  {
    bool ret = false;
    if (scale  < (unsigned int)_nr_scale)
    {
      if (size <= _capacity[scale])
      {
        _nr_ipts[scale] = size;
        ret = true;
      }
    }
    return ret;
  }

  template <typename KEYPOINT_TYPE>
  void MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::clear()
  {
    for (int i = 0; i < _nr_scale; ++i)
    {
      _nr_ipts[i] = 0;
    }
  }
  
  template <typename KEYPOINT_TYPE>
  void MultiScaleInterestPointPyramid<KEYPOINT_TYPE>::clear(unsigned int scale)
  {
    if (scale < (unsigned int)_nr_scale)
    {
      _nr_ipts[scale] = 0;
    }
  }

}/* namespace idl */

